#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
import sys
import moveit_commander
from geometry_msgs.msg import PointStamped, PoseStamped
import threading  # 新增：用于非阻塞退出

class SimpleMoveItNode:
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('simple_moveit_node', anonymous=True)

        # 初始化机械臂控制器
        self.arm = moveit_commander.MoveGroupCommander('robot_arm')
        self.arm.set_pose_reference_frame('base_link')
        self.arm.allow_replanning(True)
        self.arm.set_goal_position_tolerance(0.001)
        self.arm.set_goal_orientation_tolerance(0.01)
        self.arm.set_max_velocity_scaling_factor(0.3)
        self.arm.set_max_acceleration_scaling_factor(0.3)
        self.end_effector_link = self.arm.get_end_effector_link()

        # 保存订阅者对象
        self.subscriber = rospy.Subscriber(
            "/yolo/object_world", 
            PointStamped, 
            self.callback
        )
        rospy.loginfo("等待目标点...")

        rospy.spin()

    def callback(self, msg: PointStamped):
        # 1. 接收一次后取消订阅，避免重复执行
        self.subscriber.unregister()
        rospy.loginfo("已取消目标点订阅，仅处理当前目标")

        rospy.loginfo(f"接收到目标点: x={msg.point.x:.3f}, y={msg.point.y:.3f}, z={msg.point.z:.3f}")

        target_pose = PoseStamped()
        target_pose.header.frame_id = 'base_link'
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = msg.point.x
        target_pose.pose.position.y = msg.point.y
        target_pose.pose.position.z = 0.11  # 固定Z高度，比如0.15米


        # 固定末端姿态
        target_pose.pose.orientation.x = -0.705094
        target_pose.pose.orientation.y = 0.70808
        target_pose.pose.orientation.z = 0.026443
        target_pose.pose.orientation.w = -0.027674

        # 机械臂规划与执行
        self.arm.set_start_state_to_current_state()
        self.arm.set_pose_target(target_pose, self.end_effector_link)
        plan_success, traj, planning_time, error_code = self.arm.plan()

        if plan_success:
            rospy.loginfo("规划成功，开始执行轨迹...")
            self.arm.execute(traj, wait=True)
            rospy.loginfo("轨迹执行完成。")
        else:
            rospy.logerr("规划失败。")

        # 2. 关键修改：仅终止当前节点，不影响MoveIt和其他节点
        rospy.loginfo("当前节点任务结束，退出终端...")
        # 用线程延迟退出，避免回调函数未完成导致的异常
        threading.Timer(0.5, self.shutdown_node).start()

    def shutdown_node(self):
        # 只关闭当前节点，不影响ROS核心和MoveIt服务
        rospy.signal_shutdown("任务完成，主动退出")
        # 退出当前进程（终端会关闭）
        sys.exit(0)

if __name__ == "__main__":
    try:
        SimpleMoveItNode()
    except rospy.ROSInterruptException:
        pass
